/*
 * main_controller.h
 *
 *  Created on: Apr 9, 2010
 *      Author: Aron
 *
 * High level controler
 *
 */
//working version

#ifndef MAIN_CONTOLLER_H_
#define MAIN_CONTOLLER_H_

#define pi 3.14159265
#define pi2 1.57079633
#define pi23 4.71238898

#include "xtask.h"
#include "xqueue.h"
#include "log.h"
#include "ir_manager.h"
#include "robo_control.h"
#include <cmath>


using namespace kthrobot;
using namespace std;

namespace Group5 {

    // Used to define the strategy of the controller
    enum Direction {
        FORWARD,
        LEFT,
        RIGHT,
        BACKWARD
    };
    class Order {
      public:
        Order(): direction(FORWARD), next(NULL) {};
        Order(Direction dir): direction(dir), next(NULL) {};
        Order(Order& norder): direction(FORWARD), next(&norder) {};
        Order(Direction dir, Order& norder): direction(dir), next(&norder) {};
        Direction direction;
        Order* next;
    };

    class Main_Controller : public XTask {
    public:
        Main_Controller();
        virtual ~Main_Controller();
        void TaskCode();
        
        void SetMode(int m);

        // Mapping of the events in the queue
        static const int LOGIC      =  0;
        static const int STOP       =  1;
        static const int TURNLEFT   =  2;
        static const int TURNRIGHT  =  3;
        static const int FORWARD40  =  4;
        static const int UTURN      =  5;
        static const int DUMBFORWARD = 6;
        static const int CORRECT    = 7;

        static const int BALLCATCHING = 0;
        static const int EXPLORING    = 1;

        double ang;


    private:
        double CalcRelTargetSpeed();
        double GetCorrectionOneWall(int front, int rear);
        double GetCorrectionBothWalls();
        double GetRangeCorrOneWall(int front, int rear);
        double GetRangeCorrBothWalls();
        bool MotorsStopped();
        double GetDeltaAngle();
        void ResetAngBothWalls();
        double GetClosestStraightAng();

        static const int hz_ = 10;
        bool can_move_forward_;
        bool can_move_left_;
        bool can_move_right_;
        int mode_;

        static const int BASE_SPEED = 2;
        static const int MAXWIDTH = 100;

        static const bool USE_CAMERA = true;
        static const bool USE_MOTORS = true;

        // Log options
        static const bool LOG_STATES = true;
        static const bool LOG_MULTIPLE_SIDE_DETECTIONS = false;
        static const bool LOG_OPENINGS = true;
        static const bool LOG_RESET_ANGLE = false;
        static const bool LOG_DECISIONS = true;


        double current_rspeed;
        double current_lspeed;

        SensorsReader sensors_reader_;
        IRManager ir_manager_;
        Robo_Control motor1_, motor2_;
    };
};

#endif /* MAIN_CONTOLLER_H_ */
